#include <nice2d/util.h>
#include <nice2d/components/flocking.h>

class FlockingData
{
public:
    sf::Vector2f seek(const sf::Vector2f &target, std::shared_ptr<MovingSprite> sprite)
    {
        auto desired = target - getRectCenter(sprite->getBoundingBox());
        normalizeVector2f(desired);
        desired *= maxSpeed;
        return desired;
    }

    sf::Vector2f cohesion(std::shared_ptr<MovingSprite> sprite)
    {
        sf::Vector2f steer;

        int count = 0;
        for (auto &item : boids) {
            if (item == sprite)
                continue;
            float distance = length(getRectCenter(sprite->getBoundingBox()) - getRectCenter(item->getBoundingBox()));
            if (distance < maxNeighbordDistance) {
                steer += getRectCenter(item->getBoundingBox());
                count++;
            }
        }

        if (count > 0) {
            steer /= float(count);
            return seek(steer, sprite);
        } else
            return sf::Vector2f();
    }

    sf::Vector2f separation(std::shared_ptr<MovingSprite> sprite)
    {
        sf::Vector2f steer;
        int count = 0;

        for (const auto &item : boids) {
            if (item != sprite) {
                float distance = length(getRectCenter(item->getBoundingBox()) - getRectCenter(sprite->getBoundingBox()));
                if (distance < minSeparation) {
                    auto diff = getRectCenter(sprite->getBoundingBox()) - getRectCenter(item->getBoundingBox());
                    normalizeVector2f(diff);
                    steer += diff;
                    count++;
                }
            }
        }

        if (count > 0) {
            steer /= (float)count;
        }

        if (length(steer) > 1.0f) {
            normalizeVector2f(steer);
            steer *= maxSpeed;
            steer -= sprite->getVelocity();
            scaleVector2f(steer, maxForce);
        }

        return steer;
    }

    sf::Vector2f alignment(std::shared_ptr<MovingSprite> sprite)
    {
        sf::Vector2f steer;

        int count = 0;
        for (auto &item : boids) {
            float distance = length(getRectCenter(item->getBoundingBox()) - getRectCenter(sprite->getBoundingBox()));
            if (item != item && distance < maxNeighbordDistance) {
                auto vel = item->getVelocity();
                normalizeVector2f(vel);
                steer += vel;
                count++;
            }
        }

        if (count > 0) {
            steer /= (float)count;
            normalizeVector2f(steer);
            steer *= maxSpeed;
            steer = steer - sprite->getVelocity();
            scaleVector2f(steer, maxForce);
            return steer;
        } else {
            return steer;
        }
    }

    void applyBorder()
    {
        if (!border.has_value())
            return;

        auto area = border.value();

        auto itr = boids.begin();
        while (itr != boids.end()) {
            auto box = (*itr)->getBoundingBox();
            sf::Vector2f position = (*itr)->getPosition();
            if (position.x-box.width < area.left)
                position.x = area.left + area.width;
            if (position.y-box.height < area.top)
                position.y = area.top + area.height;
            if (position.x > area.left + area.width)
                position.x = area.left-box.width;
            if (position.y > area.top + area.height)
                position.y = area.top-box.height;

            (*itr)->setPosition(position);
            itr ++;
        }
    }

    void applyBehaviour(int time)
    {
        sf::Vector2f rule1, rule2, rule3;

        for (auto &item : boids) {
            rule1 = cohesion(item);
            rule2 = separation(item);
            rule3 = alignment(item);

            auto vel = rule1 + rule2 + rule3;
            normalizeVector2f(vel);
            scaleVector2f(vel, maxSpeed);

            vel += item->getVelocity();
            item->setVelocity(vel);
        }
    }

    MovingSprites boids;
    float maxSpeed = 12.0f;
    float maxForce = 120.0f;
    float maxNeighbordDistance = 128.0f;
    float minSeparation = 64.0;
    std::optional<sf::FloatRect> border;
};

Flocking::Flocking(NodePointer owner):
    Component(owner),
    data(new FlockingData())
{
}

Flocking::~Flocking()
{
}

void Flocking::setSprites(const MovingSprites &sprites)
{
    data->boids = sprites;
}

void Flocking::setMaxSpeed(float speed)
{
    data->maxSpeed = speed;
}

float Flocking::getMaxSpeed() const
{
    return data->maxSpeed;
}

void Flocking::setMaxForce(float force)
{
    data->maxForce = force;
}

float Flocking::getMaxForce() const
{
    return data->maxForce;
}

void Flocking::setMaxNeighbordDistance(float distance)
{
    data->maxNeighbordDistance = distance;
}

float Flocking::getMaxNeighbordDistance() const
{
    return data->maxNeighbordDistance;
}

void Flocking::setMinSeparation(float separation)
{
    data->minSeparation = separation;
}

float Flocking::getMinSeparation() const
{
    return data->minSeparation;
}

void Flocking::setBorderArea(const sf::FloatRect &area)
{
    data->border = area;
}

PTree Flocking::serialize()
{
    PTree node;
    node.add<float>("maxSpeed", getMaxSpeed());
    node.add<float>("maxForce", getMaxForce());
    node.add<float>("minSeparation", getMinSeparation());
    node.add<float>("maxNeighbordDistance", getMaxNeighbordDistance());
    if (data->border.has_value())
        node.add_child("border",serializeObject(data->border.value()));
    return node;
}

void Flocking::deserialize(const PTree &node)
{
    setMaxSpeed(node.get<float>("maxSpeed"));
    setMaxForce(node.get<float>("maxForce"));
    setMinSeparation(node.get<float>("minSeparation"));
    setMaxNeighbordDistance(node.get<float>("maxNeighbordDistance"));
    auto border = node.get_child_optional("border");
    if(border.has_value())
        data->border = deserializeFloatRect(border.value());
}

void Flocking::update(float time)
{
    data->applyBehaviour(time);
    data->applyBorder();
}
